#include "./motor.h"
#include "../device/device.h"
#include "../rtx/rtx_os.h"

namespace driver {

utils::Object<Motor_t> motor;

// PA5 <=> Left_H
// PA6 <=> Left_L
// PA9 <=> Right_H
// PA10 <=> Right_L
Motor_t::Motor_t() : m_dir(false) 
{
    GPIO_InitStruType gpio;

    gpio.GPIO_Signal = GPIO_Pin_Signal_Digital;
    gpio.GPIO_Func = GPIO_Func_0;
    gpio.GPIO_Direction = GPIO_Dir_Out;
    gpio.GPIO_PUEN = GPIO_PUE_Input_Disable;
    gpio.GPIO_PDEN = GPIO_PDE_Input_Disable;
    gpio.GPIO_OD = GPIO_ODE_Output_Disable;
    gpio.GPIO_DS = GPIO_DS_Output_Strong;
    GPIO_Init(GPIOA, GPIO_Pin_5, &gpio);
    GPIOA_ResetBit(GPIO_Pin_5);

    GPIO_Init(GPIOA, GPIO_Pin_6, &gpio);
    GPIOA_SetBit(GPIO_Pin_6);

    GPIO_Init(GPIOA, GPIO_Pin_9, &gpio);
    GPIOA_ResetBit(GPIO_Pin_9);

    GPIO_Init(GPIOA, GPIO_Pin_10, &gpio);
    GPIOA_SetBit(GPIO_Pin_10);

    set_dir(Release);

    gpio.GPIO_Direction = GPIO_Dir_In;
    GPIO_Init(GPIOA, GPIO_Pin_3, &gpio);
    GPIO_Init(GPIOA, GPIO_Pin_4, &gpio);

    KINT_InitSettingType x;
    x.Trigger_Style = KINT_Trigger_Rising_Edge;
    x.IE_Set = KINT_IE_Set_Enable;
    KINT_Init(GPIOA, GPIO_Pin_3, &x);
  //  KINT_Init(GPIOA, GPIO_Pin_4, &x);
}

utils::Error_t Motor_t::run3() {
    utils::Error_t err;
    for (uint8_t i = 0; i < 3; i ++) {
        err = run();
        if (err.is_ok()) {
            return err;
        }
    }
    return err;
}

utils::Error_t Motor_t::run() {
    uint32_t time_count = 0;
    utils::Error_t err;
    clr_flag();

    if (m_dir) {
        set_dir(CW);
    } else {
        set_dir(CCW);
    }
    m_dir = !m_dir;

    for (;;) {
        osDelay(100);
        if (is_raster_edge()) {
            break;
        }
        time_count += 100;
        if (time_count >= Timeout) {
            err = utils::ec::MotorTimeout;
            break;
        }
    }
    set_dir(Release);
    m_err = err;
    return err;
}

bool Motor_t::is_checked() {
    return GPIO_ReadBit(GPIOA, GPIO_Pin_3) != RESET;
}

bool Motor_t::is_checked2() {
    return GPIO_ReadBit(GPIOA, GPIO_Pin_4) == RESET;
}

void Motor_t::set_dir(Dir_t dir) {
    
    switch (dir) {
        case CW:
            GPIOA_SetBit(GPIO_Pin_5);
            GPIOA_SetBit(GPIO_Pin_6);
            GPIOA_ResetBit(GPIO_Pin_9);
            GPIOA_ResetBit(GPIO_Pin_10);
        break;

        case CCW:
            GPIOA_SetBit(GPIO_Pin_9);
            GPIOA_SetBit(GPIO_Pin_10);
            GPIOA_ResetBit(GPIO_Pin_5);
            GPIOA_ResetBit(GPIO_Pin_6);
        break;

        case Brake:
            GPIOA_ResetBit(GPIO_Pin_5);
            GPIOA_ResetBit(GPIO_Pin_9);
            GPIOA_ResetBit(GPIO_Pin_6);
            GPIOA_ResetBit(GPIO_Pin_10);
        break;

        case Release:
            GPIOA_ResetBit(GPIO_Pin_5);
            GPIOA_ResetBit(GPIO_Pin_9);
            GPIOA_SetBit(GPIO_Pin_6);
            GPIOA_SetBit(GPIO_Pin_10);
        break;
    }
    osDelay(100);
}

}

using namespace driver;

extern "C"
void KINT_IRQHandler() {
    if (KINT_GetITFlag(GPIOA, GPIO_Pin_3) != RESET) {
        KINT_ClearITFlag(GPIOA, GPIO_Pin_3);
    }
    // if (KINT_GetITFlag(GPIOA, GPIO_Pin_4) != RESET) {
    //     KINT_ClearITFlag(GPIOA, GPIO_Pin_4);
    // }
    motor->set_flag();
}
